#include <Arduino.h>
#include "ProtocolParser.h"
#include "BluetoothHandle.h"
#include "debug.h"
#include "JoyRobot.h"

volatile int ufront;
volatile int ileft;
volatile int iright;
volatile int uleft;
volatile int uright;
volatile int Speed;
volatile boolean AutoRun;

ProtocolParser *mProtocol = new ProtocolParser();
JoyRobot hbot(mProtocol, EM_IN1_PIN, EM_IN2_PIN, EM_IN3_PIN, EM_IN4_PIN);
//byte Ps2xStatus, Ps2xType;
unsigned long last_read;

int ServoPinLeft = 7; // 左 180
int ServoPinRight = 8; // 右 0
int lastAuto = 1; // 1 前进 2后退 3转左 4 转右

void setup()
{
  Serial.begin(9600);
  hbot.init();
  hbot.SetMotorPin(6, 10, 5, 9);
  hbot.SetInfraredAvoidancePin(12, A5);
  hbot.SetRGBUltrasonicPin(3, 2);
  hbot.SetInfraredTracingPin(A2, A1, A0);
  hbot.SetServoPin(13);

  hbot.SetSpeed(0);
  //  hbot.SetControlMode((E_SMARTCAR_CONTROL_MODE)(0));
  //  Ps2xType = hbot.mPs2x->readType();
  //
  //  hbot.SetControlMode((E_SMARTCAR_CONTROL_MODE)8);

  hbot.IrInit();


  hbot.mRgbUltrasonic->SetServoBaseDegree(90);
  hbot.mRgbUltrasonic->SetServoDegree(90);

  ufront = 0;
  ileft = 0;
  iright = 0;
  uleft = 0;
  uright = 0;
  Speed = 70;
  AutoRun = false;
}

void loop()
{
  byte keyCode = hbot.mIrRecv->getCode();
  if (hbot.mIrRecv->irKeyPressed())
  {
    last_read = millis();
    Serial.println("irKeyPressed");
    // DEBUG_LOG(DEBUG_LEVEL_INFO, "irKeyCode = %lx \n", keyCode);
    // DEBUG_LOG(DEBUG_LEVEL_INFO, "mIrRecv =%s \n", keyCode);
    switch (keyCode)
    {
      case EM_IR_KEYCODE_UP:
        Serial.println("EM_IR_KEYCODE_UP");
        hbot.SetSpeed(Speed);
        hbot.Move(1);
        break;
      case EM_IR_KEYCODE_DOWN:
        Serial.println("EM_IR_KEYCODE_DOWN");
        hbot.SetSpeed(Speed);
        hbot.Move(2);
        break;
      case EM_IR_KEYCODE_LEFT:
        Serial.println("EM_IR_KEYCODE_LEFT");
        hbot.SetSpeed(Speed);
        hbot.TurnLeft();
        break;
      case EM_IR_KEYCODE_RIGHT:
        Serial.println("EM_IR_KEYCODE_RIGHT");
        hbot.SetSpeed(Speed);
        hbot.TurnRight();
        break;
      case EM_IR_KEYCODE_A:
        Serial.println("EM_IR_KEYCODE_A");
        AutoRun = true;
        break;
      case EM_IR_KEYCODE_C:
        Serial.println("EM_IR_KEYCODE_C");
        AutoRun = false;
        hbot.SetSpeed(0);
        hbot.KeepStop();
        break;
      case EM_IR_KEYCODE_PLUS:
        Speed = Speed + 5;
        hbot.SetSpeed(Speed);
        hbot.Move(1);
        break;
      case EM_IR_KEYCODE_REDUCE:
        Speed = Speed - 5;
        hbot.SetSpeed(Speed);
        hbot.Move(1);
        break;
      case EM_IR_KEYCODE_1:
        // 收起太阳能板
        hbot.SetServoExtDegree(ServoPinLeft, 90);  // 左
        hbot.SetServoExtDegree(ServoPinRight, 90); // 右
        hbot.ServoExtDetach(1000);
        break;
      case EM_IR_KEYCODE_2:
        // 撑开太阳能板
        hbot.SetServoExtDegree(ServoPinLeft, 180);  // 左
        hbot.SetServoExtDegree(ServoPinRight, 0); // 右
        hbot.ServoExtDetach(1000);
        break;
      case EM_IR_KEYCODE_7:
        hbot.mRgbUltrasonic->SetServoDegree(20);
        break;
      case EM_IR_KEYCODE_8:
        hbot.mRgbUltrasonic->SetServoDegree(90);
        break;
      case EM_IR_KEYCODE_9:
        hbot.mRgbUltrasonic->SetServoDegree(180);
        break;
      default:
        Serial.println("default");
        if (!AutoRun)
        {
          hbot.SetSpeed(0);
          hbot.KeepStop();
        }
        break;
    }

    delay(100);
  }
  else
  {
    //Serial.println("no key");
    if (AutoRun)
    {
      ufront = hbot.GetUltrasonicValue(0);
      ileft = hbot.GetInfraredAvoidanceValue(0);
      iright = hbot.GetInfraredAvoidanceValue(1);
      delay(20);
      Serial.print("ufront: ");
      Serial.println(ufront);
      switch (hbot.mInfraredTracing->GetValue())
      {
        case 0b111:
          if (ufront < 8 || (iright == 0 && ileft == 0 ))
          {
            // 停止
            hbot.SetSpeed(0);
            hbot.KeepStop();
            uleft = hbot.GetUltrasonicValue(1);
            uright = hbot.GetUltrasonicValue(2);
            if (lastAuto == 4 || ( uright >= uleft && lastAuto != 3))
            {
              lastAuto = 4; // 1 前进 2后退 3转左 4 转右
              hbot.SetSpeed(50);
              hbot.TurnRight();
              delay(400);
            } else if (lastAuto == 3 || (uright < uleft && lastAuto != 4))
            {
              lastAuto = 3; // 1 前进 2后退 3转左 4 转右
              hbot.SetSpeed(50);
              hbot.TurnLeft();
              delay(400);
            } else {
              lastAuto = 5; // 1 前进 2后退 3转左 4 转右 5 掉头
              hbot.SetSpeed(50);
              hbot.TurnRight();
              delay(1300);
            }
            hbot.SetSpeed(0);
            hbot.KeepStop();
          } else {
            lastAuto = 1; //前进
            if (iright != 0 && ileft == 0)
            {
              hbot.SetSpeed(50);
              hbot.Drive(10);
            }
            else if (iright == 0 && ileft != 0)
            {
              hbot.SetSpeed(50);
              hbot.Drive(170);
            }
            else
            {
              hbot.SetSpeed(40);
              hbot.GoForward();
            }
          }
          break;
        default:
          hbot.SetSpeed(0);
          hbot.KeepStop();
          break;
      }
    }
    else
    {
      // delay(200);
      long temp = millis() - last_read;
      if (temp > 200)
      {
        hbot.SetSpeed(0);
        hbot.KeepStop();
      }
    }
  }
}
